/* motor_balance_ctrl.c */

#include "motor_balance_ctrl.h"
#include "math.h"
#include <stdio.h>
#include <stdlib.h>

/* 获取电机当前转速值 */
void Motor_GetSpeedPulse(Motor_EncCntVal_T *output)
{
    output->LeftEncCntVal  = MOTOR_PWM_COUNTER_MAX - Motor_GetQuadDecoderValue(MOTOR_ID_A);
    output->RightEncCntVal = MOTOR_PWM_COUNTER_MAX - Motor_GetQuadDecoderValue(MOTOR_ID_B);

    Motor_ClearQuadDecoderValue(MOTOR_ID_A | MOTOR_ID_B);  // 获取转速数据后，清除计数器

    /* 电机反转时，转速计数器值是从最大值往下递减的，需要将读数减去最大值转换成负数 */
    if (output->LeftEncCntVal >= MOTOR_PWM_COUNTER_MAX/2)
    {
        output->LeftEncCntVal -= MOTOR_PWM_COUNTER_MAX;
    }
    if (output->RightEncCntVal >= MOTOR_PWM_COUNTER_MAX/2)
    {
        output->RightEncCntVal -= MOTOR_PWM_COUNTER_MAX;
    }
}

/* 把目标速度和当前转速值输入PI控制器，计算出速度环PWM控制量 */
float Motor_SpeedPwmControl(Motor_EncCntVal_T *input, float TargetSpeed, bool MotorStopFlag)
{
    static float fVdelta=0.0f;
    static float fV=0.0f;
    static float fP=0.0f, fI=0.0f;
    static float PwmOut;

    fVdelta = input->LeftEncCntVal + input->RightEncCntVal + MOTOR_ENC_COMPENSATION;

    fV *= (float)0.7f;
    fV += fVdelta*(float)0.3f;

    fI += fV;
    fI -= TargetSpeed;

    if(fI > SPEED_INTEGRAL_MAX) {fI = SPEED_INTEGRAL_MAX;}
    if(fI < SPEED_INTEGRAL_MIN) {fI = SPEED_INTEGRAL_MIN;}

    fP = fV*SPEED_P;
    PwmOut = fP + fI*SPEED_I;

    if(MotorStopFlag == true)
    {
        fI = 0;  //电机如果被关闭，清除积分
    }
    return(PwmOut);
}

/* 把角度值和X轴向角速度值输入PD控制器，计算出角度环PWM控制量 */
float Motor_AnglePwmControl(float KalmanAngle, float GyroX)
{
    static float Bias;
    static float PwmOut;

    Bias = KalmanAngle - ANGLE_OFFSET;
    PwmOut = ANGLE_P*Bias + ANGLE_D*GyroX;

    return(PwmOut);
}

/* 把目标转向速度和Z轴向角速度值输入PD控制器，计算出转向PWM控制量 */
float Motor_TurnPwmControl(float TargetTurn, float GyroZ)
{
    static float PwmOut;
    if(fabs(TargetTurn) < 10.0f)
    {
        PwmOut = TargetTurn*TURN_P + GyroZ*TURN_D;               
    }
    else if(fabs(TargetTurn) < 20.0f)
    {
        PwmOut = TargetTurn*TURN_P + GyroZ*TURN_D;               
    }
    else
    {
        PwmOut = TargetTurn*TURN_P + GyroZ*TURN_D;
    }           
    
    return (PwmOut);
}    

/* 把速度环，角度环和转向PWM控制量叠加后，更新至PWM控制器 */
void Motor_RefreshPwmController(float AnglePwmOut, float SpeedPwmOut, float TurnPwmOut)
{
    static int32_t LeftPwmCtrlVal, RightPwmCtrlVal;

    LeftPwmCtrlVal  = (-AnglePwmOut) + SpeedPwmOut + TurnPwmOut;
    RightPwmCtrlVal = (-AnglePwmOut) + SpeedPwmOut - TurnPwmOut;

    if(LeftPwmCtrlVal < -MOTOR_PWM_MAX_LIMIT) LeftPwmCtrlVal = -MOTOR_PWM_MAX_LIMIT;
    if(LeftPwmCtrlVal > MOTOR_PWM_MAX_LIMIT) LeftPwmCtrlVal = MOTOR_PWM_MAX_LIMIT;
    if(RightPwmCtrlVal < -MOTOR_PWM_MAX_LIMIT) RightPwmCtrlVal = -MOTOR_PWM_MAX_LIMIT;
    if(RightPwmCtrlVal > MOTOR_PWM_MAX_LIMIT) RightPwmCtrlVal = MOTOR_PWM_MAX_LIMIT;

    /* Update the PWM for speed. */
#if 1
    if (LeftPwmCtrlVal >= 0)
    {
        Motor_SetTurnDirection(MOTOR_ID_A, eMotor_DirectionForward);
        Motor_SetTrunSpeed(MOTOR_ID_A, LeftPwmCtrlVal+MOTORA_DEAD_PWM);
    }
    else /* LeftPwmCtrlVal < 0 */
    {
        Motor_SetTurnDirection(MOTOR_ID_A, eMotor_DirectionBackward);
        Motor_SetTrunSpeed(MOTOR_ID_A, (-LeftPwmCtrlVal)+MOTORA_DEAD_PWM);
    }
#endif
#if 1
    if (RightPwmCtrlVal >= 0)
    {
        Motor_SetTurnDirection(MOTOR_ID_B, eMotor_DirectionForward);
        Motor_SetTrunSpeed(MOTOR_ID_B, RightPwmCtrlVal+MOTORB_DEAD_PWM);
    }
    else
    {
        Motor_SetTurnDirection(MOTOR_ID_B, eMotor_DirectionBackward);
        Motor_SetTrunSpeed(MOTOR_ID_B, (-RightPwmCtrlVal)+MOTORB_DEAD_PWM);
    }
#endif
}

/* 当小车倾角超过50度，电池电量低于20%以及检测到小车被拿起时，将马达强制停机 */
bool Motor_Stop(float KalmanAngle, uint32_t BatteryPercentage, bool PickUpFlag)
{
    if(    (fabs(KalmanAngle) > 50.0)
        || (BatteryPercentage < 20)
        || (PickUpFlag == true)  )
    {
        Motor_EnableTurn(false);
        Motor_SetTrunSpeed(MOTOR_ID_A, 0);
        Motor_SetTrunSpeed(MOTOR_ID_A, 0);
        return (true);
    }
    else
    {
        Motor_EnableTurn(true);
        return (false);
    }
}

/* 检测小车被拿起 */
bool Car_DetectPickUp(int16_t AccZ, float KalmanAngle, Motor_EncCntVal_T *input)
{
    static uint8_t StepFlag = 0U, count = 0U;

    /* 第一步，检测小车速度在一定时间内是否接近0。如果是进入第二步检测，否则继续监测速度 */
    if (StepFlag == 0U)
    {
        if(  abs((int)(input->LeftEncCntVal + input->RightEncCntVal)) < 20U  )
        {
            count++;
        }
        else
        {
            count = 0U;
        }
        if(count > 10U)
        {
            StepFlag = 1U;
            count = 0U;
        }
    }
    /* 第二步，检测小车是否处于垂直状态并且被向上拿起，如果是进入第三步检测，如果在2s内没有进入第三部则回到第一步 */
    if (StepFlag == 1)
    {
        if (++count > 200)
        {
            count = 0;
            StepFlag = 0;
        }
        if ((AccZ > 24000) && (KalmanAngle > (float)(-20+ANGLE_OFFSET)) && (KalmanAngle < (float)(20+ANGLE_OFFSET)))
        {
            StepFlag = 2;
            count = 0;
        }
    }
    /* 第三步，检测小车被拿起后，转速是否由于正反馈达到最大值。如果是返回真，如果超过1s条件未成立则返回第一步 */
    if (StepFlag == 2)
    {
        if (++count > 100)
        {
            count = 0;
            StepFlag = 0;
        }
        if (abs(input->LeftEncCntVal + input->RightEncCntVal) > 90)
        {
            StepFlag = 0;
            count =0;
            return (true);
        }
    }

    return (false);
}

/* 检测小车是否被放下 */
bool Car_DetectPutDown(bool MotorStopFlag, float KalmanAngle, Motor_EncCntVal_T *input)
{
    static uint8_t StepFlag=0, count=0;

    /* 马达不在停止状态，关闭检测 */
    if(MotorStopFlag == false)
    {
        return (false);
    }
    /* 检测小车是否处于垂直状态，并且电机转速为0。如果是这进入第二步。 */
    if(StepFlag == 0)
    {
        if (   (KalmanAngle > (float)(-10+ANGLE_OFFSET))
            && (KalmanAngle < (float)(10+ANGLE_OFFSET))
            && (input->LeftEncCntVal == 0)
            && (input->RightEncCntVal == 0) )
        {
            StepFlag = 1;
        }
    }
    /* 检测小车车轮是否被认为加速，如果是返回真，说明小车被放下。 */
    if (1U == StepFlag)
    {
        if (++count > 50)
        {
            StepFlag = 0;
            count = 0;
        }
        if (   (abs(input->LeftEncCntVal) > 3)
            && (abs(input->RightEncCntVal) > 3)
            && (abs(input->LeftEncCntVal) < 20)
            && (abs(input->RightEncCntVal) < 20)   )
        {
            StepFlag = 0;
            count = 0;
            return (true);
        }
    }

    return (false);
}

/* EOF. */

